#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  arm_con,        sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  led1,           sensorDigitalOut)
#pragma config(Sensor, dgtl4,  led2,           sensorDigitalOut)
#pragma config(Sensor, dgtl6,  dataout,        sensorDigitalOut)
#pragma config(Sensor, dgtl7,  datain,         sensorDigitalIn)
#pragma config(Sensor, dgtl10, LED_Green,      sensorDigitalOut)
#pragma config(Sensor, dgtl11, LED_Yellow,     sensorDigitalOut)
#pragma config(Sensor, dgtl12, LED_Red,        sensorDigitalOut)
#pragma config(Sensor, I2C_1,  claw_con,       sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           claw_con,      tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           arm_con,       tmotorVex393HighSpeed_MC29, openLoop, encoderPort, dgtl1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/


int count = 0;
const short leftButton = 1;
const short centerButton = 2;
const short rightButton = 4;
void waitForPress()
{
	while(nLCDButtons == 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

//Wait for Release------------------------------------------------
void waitForRelease()
{
	while(nLCDButtons != 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

int jump=1;
int a,b;
void pre_auton()

{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks
  // running between Autonomous and Driver controlled modes. You will need to
  // manage all user created tasks if set to false
  bStopTasksBetweenModes = true;

	}
	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...

int power,power3,time,time2,time3,clawtime;
int Lpower,Rpower;
int degree;

task ClawInAuto(){
	clearTimer(T3);
	while(time1[T3]<clawtime){
		motor[port9]=-power3;
	}
	motor[port1]=motor[port10]=0;
	stopTask(ClawInAuto);
}
task ArmInAuto(){
   clearTimer(T1);
   while(time1[T1]<time){ //millsecond
    motor[port2]=power;
	  motor[port3]=power;
  	motor[port7]=-power;
  	motor[port4]=power;
  	motor[port8]=-power;
  }
  motor[port2]=motor[port3]=motor[port4]=motor[port7]=motor[port8]=0;
  stopTask(ArmInAuto);
}
task ChassisInAuto(){ //All positive by default
	clearTimer(T2);
	while(time1[T2]<time2){
		motor[port5]=Lpower;
		motor[port6]=Rpower;
  }
	motor[port5]=0;
	motor[port6]=0;
	stopTask(ChassisInAuto);
}


task led(){
		while(1){
		SensorValue(LED_Yellow)=1;
		wait1Msec(50);

		SensorValue(LED_Yellow)=0;
		wait1Msec(50);
	}
}

task arm(){
	int output,degree,error,target=0,c5u,c5d;
	const int deadband = 20;
	const float kp = 3;
	while(1){
		//degree=getMotorEncoder(arm_con);
		c5u=vexRT[Btn5U];
		c5d=vexRT[Btn5D];

		while(c5u){
			motor[port2]=motor[port3]=motor[port4]=127;
			motor[port7]=motor[port8]=motor[port9]=-127;
			c5u=vexRT[Btn5U];
		}

		while(c5d){
			motor[port2]=motor[port3]=motor[port4]=-127;
			motor[port7]=motor[port8]=motor[port9]=127;
			c5d=vexRT[Btn5D];
  	}

		motor[port2]=motor[port3]=motor[port4]=output;
		motor[port7]=motor[port8]=motor[port9]=-output;
	}
}

task claw(){
	int degree,target=0,output,deadband,error,c6d,c6u;
	float kp=3;
	while(1){
		degree=getMotorEncoder(claw_con);
		c6d=vexRT[Btn6D];
		c6u=vexRT[Btn6U];
		while(c6u){
			motor[port1]=-127;
			motor[port10]=127;
			c6u=vexRT[Btn6U];
			target=getMotorEncoder(claw_con);
		}

		while(c6d){
			motor[port1]=127;
			motor[port10]=-127;
			c6d=vexRT[Btn6D];
			target=getMotorEncoder(claw_con);
		}

		error=target-degree;
		//if(deadband>=error) error=0;

		output=kp*error;

		motor[port1]=output;
		motor[port10]=-output;
	}

}
void release(){
  clawtime=1000;
  time=500;
  power=-127;
  power3=-80;
  startTask(ArmInAuto);
  wait1Msec(800);
  startTask(ClawInAuto);
  wait1Msec(800);
  clawtime=400;
  power3=-80;
  startTask(ClawInAuto);
  power=127;
  time=800;
  startTask(ArmInAuto);
  wait1Msec(600);
  //release claw

}


void autonomous1(){






}

void autonomous2(){
}

void autonomous3(){
}

void autonomous4(){
}



/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/
task autonomous()//arm=time chassis=time2 claw=clawtime
{
  // ..........................................................................
  // Insert user code here.
  // ..........................................................................

  // Remove this function call once you have "real" code.
  //AutonomousCodePlaceholderForTesting();
  switch(count){
  	case 0:
  	  autonomous1();
  	  break;
  	case 1:
  	  autonomous2();
  	  break;
  	case 2:
  	  autonomous3();
  	  break;
  	case 3:
  	  autonomous4();
  	  break;

  	default:
  	  break;

  }





  /*wait1Msec(300);

  Lpower=80;
  Rpower=-80;
  time2=600;
  clawtime=300;
  power3=-127;
  startTask(ChassisInAuto);
  startTask(ClawInAuto);

  wait1Msec(300);

  time=550;
  power=-127;
  startTask(ArmInAuto);*/

}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
  // User control code here, inside the loop
  //startTask(led);
  //startTask(arm);
  //startTask(claw);
  int degree,target=0,output,deadband,error,c6d,c6u,c5u,c5d;
	float kp=0.8;
  float main_v;
  startTask(led);
  while (true)
  {
  	displayLCDString(0,0,"Main Power:");
  	main_v= nAvgBatteryLevel/1000;
  	displayLCDNumber(1,0,nAvgBatteryLevel);
  	motor[port5]=vexRT[Ch3];
  	motor[port6]=-vexRT[Ch2];

		c6d=vexRT[Btn6D];
		c6u=vexRT[Btn6U];
		c5u=vexRT[Btn5U];
		c5d=vexRT[Btn5D];
  	degree=getMotorEncoder(claw_con);


		if(c6u&&!c6d){
			motor[port9]=127;
			//motor[port10]=127;
			target=getMotorEncoder(claw_con);
		}
		else if(c6d){
			motor[port9]=-127;
			//motor[port10]=-127;
			target=getMotorEncoder(claw_con);
		}
		else{
		error=target-degree;
		//if(deadband>=error) error=0;
		output=kp*error;
		motor[port9]=-output;
		//motor[port10]=0;
		}

		if(c5u&&!c5d){
			motor[port2]=motor[port3]=motor[port4]=127;
			motor[port7]=motor[port8]=-127;
		}
		else if(c5d){
			motor[port2]=motor[port3]=motor[port4]=-127;
			motor[port7]=motor[port8]=127;
  	}
		else{
			motor[port2]=motor[port3]=motor[port4]=0;
		  motor[port7]=motor[port8]=0;
		}


  	bLCDBacklight = true;
    clearLCDLine(0);
    clearLCDLine(1);

    if(SensorValue[datain]==1) count=3;
    else count=0;

    if(count) {displayLCDString(0,6,"Auto2"); turnLEDOn(LED_Red);}
    else {displayLCDString(0,6,"Auto1"); turnLEDOff(LED_Red);}





    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
    // ........................................................................

    // Remove this function call once you have "real" code.



  }
}
